#include <fstream>

#include "../tools/Log.h"
#include "Map.h"

#include <assert.h>

using namespace std;

const unsigned long SUBSPACE_RESOLUTION = 1000;

Map::Map() : name(), terrain(), terrainDraw()
{
	terrainDraw.DisableDraw();
}

bool Map::load_from_file(string file_name)
{
	int height, width;
	ifstream fin(file_name.c_str());
	
	if (!fin) {
		lout << "WARNING: Map - Unable to load map file: \"" << file_name << "\"" << endl;	
		return false;
	}

	fin >> height >> width;
	terrain = terrainVec(height, vector <int> (width, 0));
	
	for (int i = 0; i < height; i ++)
		for (int j = 0; j < width; j ++)
			fin >> terrain[i][j];
		
	terrainDraw.setTerrain(&terrain);

	return true;
}

fVec3D Map::getMapEulerAngles(unsigned long X, unsigned long Y) const {
	using namespace std;
	fVec3D n = terrainDraw.getNormal(X,Y);
	fVec3D rot;	
	
	rot.y = asin(-n.x);
	if(abs(cos(rot.y)) != 0) {
		rot.x = asin(n.y / cos(rot.y));
		rot.z = -atan(tan(rot.x) * -sin(rot.y) );
	}
	
	rot *= -180 / 3.1415926;

	return rot;
}


//TODO	Optimize the heck out of this. It's very inefficient
vector < pair <lVec2D, double> > Map::get_adjacent_positions(lVec2D p) const
{
	vector < pair<lVec2D, double> > adjacent;
	lVec3D currPos(p.x, p.y, getHeight(p));
	
	p /= SUBSPACE_RESOLUTION;
	for(int i = -1; i <= 1; i++)
	{
		for(int j = -1; j <= 1; j++) {
			if(i == 0 && j == 0)	continue;
			if(p.x+i < 0 || p.x+i >= (signed)terrain.size() ||
			   p.y+j < 0 || p.y+j >= (signed)terrain[0].size())
				continue;
			
			pair<lVec2D, double> n;
			unsigned long x = (p.x + i) * SUBSPACE_RESOLUTION;
			unsigned long y = (p.y + j) * SUBSPACE_RESOLUTION;
			unsigned long z = getHeight(x, y);
			lVec3D newPos(x,y,z);
			n.second = (currPos - newPos).magnitude();
			n.first = lVec2D(x,y);
			
			adjacent.push_back(n);
		}
	}
	
	return adjacent;
}

double Map::get_estimated_distance(lVec2D a, lVec2D b) const
{
	lVec3D A(a), B(b);
	A.z = getHeight(A.x, A.y);
	B.z = getHeight(B.x, B.y);
	return (A - B).magnitude();
}

unsigned long Map::getHeight(unsigned long x, unsigned long y) const {
	return terrainDraw.getHeight(x, y);
}

unsigned long Map::getHeight(const lVec2D& p) const {
	return getHeight(p.x, p.y);
}

bool Map::traceRayToMap(const fVec3D& p, const fVec3D& dir, lVec3D& where) {
	return terrainDraw.traceRayToMap(p,dir,where);
}


